#!/usr/bin/env python3
# -*- coding: utf-8 -*-
#做实验用三轴力配合gravity_compensation2.py
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import WrenchStamped
import serial
import time
import threading

# 一次性读取 8 路
cmd_all = bytes([0xFE, 0x04, 0x00, 0x00, 0x00, 0x08, 0xE5, 0xC3])

def parse_value(high, low):
    val = (high << 8) | low
    if val > 50000:
        val = val - 65535
    return val / 100.0

def parse_frame(data):
    if len(data) < 21 or data[0] != 0xFE or data[1] != 0x04:
        return None
    vals = []
    for i in range(8):
        high = data[3 + 2*i]
        low  = data[4 + 2*i]
        vals.append(parse_value(high, low))
    return vals

class ForcePublisher(Node):
    def __init__(self):
        super().__init__('force_data_node')
        self.publisher_ = self.create_publisher(WrenchStamped, '/force_data', 10)
        self.ser = serial.Serial("/dev/ttyACM0", 19200, timeout=0.05)
        self.z_offset = None
        self.y_offset = None
        self.x_offset = None
        self.prev_vals = None

        self.running = True
        self.thread = threading.Thread(target=self.read_loop, daemon=True)
        self.thread.start()

    def read_loop(self):
        while self.running:
            try:
                self.ser.write(cmd_all)
                time.sleep(0.004)  # 等待完整帧返回
                data = self.ser.read(25)
                vals = parse_frame(data)
                if not vals:
                    continue

                # 数据没变就跳过
                if vals == self.prev_vals:
                    continue
                self.prev_vals = vals

                x_val, y_val, z_val = vals[0], vals[1], vals[2]

                if self.z_offset is None:
                    self.z_offset = z_val
                    self.get_logger().info(f"Z轴去皮完成, 偏移={self.z_offset:.2f}")

                z_val -= self.z_offset
                
                if self.y_offset is None:
                    self.y_offset = y_val
                    self.get_logger().info(f"Y轴去皮完成, 偏移={self.y_offset:.2f}")

                y_val -= self.y_offset
                
                if self.x_offset is None:
                    self.x_offset = 6.5
                    self.get_logger().info(f"Y轴去皮完成, 偏移={self.x_offset:.2f}")

                x_val -= self.x_offset

                msg = WrenchStamped()
                msg.header.stamp = self.get_clock().now().to_msg()
                msg.header.frame_id = "base_link"
                msg.wrench.force.x = x_val
                msg.wrench.force.y = y_val
                msg.wrench.force.z = z_val
                self.publisher_.publish(msg)

                self.get_logger().info(f"Fx={x_val:.2f}, Fy={y_val:.2f}, Fz={z_val:.2f}")

            except Exception as e:
                self.get_logger().warn(f"串口异常: {e}")
                time.sleep(0.01)

    def destroy_node(self):
        self.running = False
        if self.thread.is_alive():
            self.thread.join(timeout=1.0)
        if self.ser.is_open:
            self.ser.close()
        super().destroy_node()

def main(args=None):
    rclpy.init(args=args)
    node = ForcePublisher()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == "__main__":
    main()
